
#include <jsoncpp/json/json.h>
#include "ubt_speaker_led/speaker_led.h"
#include <std_msgs/msg/int32_multi_array.hpp>
#include <std_msgs/msg/u_int8_multi_array.hpp>
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/char.hpp>
#include <std_msgs/msg/u_int32.hpp>
#include <std_msgs/msg/u_int8.hpp>
#include <std_msgs/msg/bool.hpp>
#include <std_msgs/msg/float32_multi_array.hpp>
#include <boost/thread.hpp>
#include "ubt_interface/msg/battery_data.hpp"
#include "ubt_interface/msg/wifi_info.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <nav_msgs/msg/odometry.hpp>
#include <string>
#include <map>
#include "ubt_speaker_led/node.h"  

rclcpp::Node::SharedPtr node;
/// 本体整体状态列表
static std::map<std::string, unsigned char> g_ubt_state_list =
{
    {"UBT_WAIT",     1},
    {"UBT_ACTION",   3},
    {"UBT_OBSTACLE", 4},
    {"UBT_PAUSE",    5},
    {"UBT_INITPOSE", 6},
    {"UBT_STOP",     9},
    {"UBT_RMCTL",    10},
    {"UBT_WAITPOSE", 11},
    {"UBT_NREFAULT", 12},
};

// 任务状态列表
static std::map<std::string, unsigned int> g_process_state_list =
{
    {"TASK_PROCESS",        1 << 0},     // 任务开始执行
    {"TASK_ARRIVED",        1 << 1},     // 任务到达目标点
    {"TASK_FINISH",         1 << 2},     // 任务执行完成
    {"NAV_ABORT_EXCEED_3M", 1 << 3},     // 超过3米执行失败
    {"CHARGE_ABORT",        1 << 5},     // 对桩失败
    {"USR_CANCEL",          1 << 6},     // 任务取消
    {"NAV_TRACKFAIL",       1 << 7},     // 导航轨迹跟踪失败
    {"NAV_POSEJUMP",        1 << 8}      // 定位丢失
};

bool g_StopThread = false;
static std::string g_ubt_laststate = " ";
static const std::string g_find_string= "stopReasonDesc=";

static std::string getErrorReasonState(const std::string& desc)
{
    size_t pos = desc.rfind(g_find_string);

    if (pos + g_find_string.length() < desc.length())
    {
        std::string address = desc.substr(pos + g_find_string.length());
        return address;
    }

    return  " ";
}

static bool praseJsonString(const std::string& jsonData, Json::Value& root)
{
    JSONCPP_STRING err;
    Json::CharReaderBuilder builder;
    const std::unique_ptr<Json::CharReader> reader(builder.newCharReader());
    const auto rawJsonLength = static_cast<int>(jsonData.length());

    if (!reader->parse(jsonData.c_str(), jsonData.c_str() + rawJsonLength, &root, &err))
    {
        RCLCPP_INFO_STREAM(node->get_logger(),"json prase error, data: " << jsonData << " err info: " << err);
        return false;
    }

    return true;
}

static void Update_Charge_Status(const std::string &state)
{
    if (state == "UBT_CHARGE" || state == "UBT_CHARGEMANUAL")
    {
        Speaker_Led::Instance()->chargestatus = 1;
    }
    else if (state == "UBT_WAIT" || state == "UBT_WAITPOSE" || state == "UBT_STOP" ||
             state == "UBT_RMCTL" || state == "UBT_BREAKOPENED" || state == "UBT_RUN" ||
             state == "UBT_DRAGUP")
    {
        if (Speaker_Led::Instance()->chargestatus == 1)
        {
            Speaker_Led::Instance()->chargestatus = 2;
        }
    }
}

static void Update_Ubt_Status(const std::string &state)
{
    Speaker_Led* ph = Speaker_Led::Instance();

    if (g_ubt_laststate != state)
    {
        RCLCPP_INFO_STREAM(node->get_logger(),"last state: " << g_ubt_laststate << ", current state: " << state);
    }
    if (g_ubt_state_list.find(state) != g_ubt_state_list.end())
    {
        ph->ubtstate = g_ubt_state_list[state];
    }
    else if (state == "UBT_RUN" && g_ubt_laststate == "UBT_WAIT")
    {
        ph->ubtstate = 2;
    }
    else if (state == "UBT_RECHARGE" && g_ubt_laststate == "UBT_WAIT")
    {
        if (Speaker_Led::Instance()->lowerpower == 1 || Speaker_Led::Instance()->lowerpower == 2) // 低电量回充
        {
            ph->ubtstate = 7;
        }
        else
        {
            ph->ubtstate = 8;
        }
    }
    else if(state == "UBT_PAUSE" || state == "UBT_PAUSEMANUAL")
    {
        ph->ubtstate = 5;
    }
    else if(state == "UBT_NREFAULT")
    {
        ph->ubtstate = 15;
    }
    else if(state == "UBT_REFAULT")
    {
        ph->ubtstate = 16;
    }
    else
    {
        ph->ubtstate = 0;
    }

    g_ubt_laststate = state;
}

/*
msgs.data:
{
    "stamp": 123,
    "state":"UBT_WAIT",
    "hw_upcode": {
        "state":1,
        "tag":123,
        "pose": {
            "x":0.102,
            "y":0.023,
            "theta":1.24
        }
    },
    "hw_liftstate": 1
}
*/
// void State_Callback(const std_msgs::msg::String::SharedPtr state)
void StateStatusCallback(const std_msgs::msg::String::SharedPtr msgs)
{
    Json::Value root;

    if (praseJsonString(msgs->data, root) == false)
    {
        return;
    }

    std::string state = root["state"].asString();
    Update_Charge_Status(state);
    Update_Ubt_Status(state);
}

void Wifi_Info_Callback(const ubt_interface::msg::WifiInfo::SharedPtr info)
{
    Speaker_Led::Instance()->wifistatus = info->netstatus;
    Speaker_Led::Instance()->Update_net_status();
    // RCLCPP_DEBUG(node->get_logger(),"Speaker_Led::Instance()->netstatus = %d\n", Speaker_Led::Instance()->netstatus);
}

void Pb_Status_Callback(const std_msgs::msg::UInt8::SharedPtr status)
{
    Speaker_Led::Instance()->pbstatus = status->data;

    if (status->data == 0) // 关机
    {
        Speaker_Led::Instance()->Speaker_Ctrl(power_off, Speaker_Led::Instance()->g_VoiceVolume);
        rclcpp::shutdown();    // 关闭该节点，避免播报其他语音
    }
}

void Ble_Status_Callback(const std_msgs::msg::UInt32::SharedPtr status)
{
    Speaker_Led::Instance()->blestatus = status->data;
}

void Event_Emergency_Status_Callback(const std_msgs::msg::UInt32::SharedPtr status)
{
    Speaker_Led::Instance()->emergency_ = status->data;
}

void Event_Collision_Status_Callback(const std_msgs::msg::UInt32::SharedPtr status)
{
    Speaker_Led::Instance()->collision_ = status->data;
}
void Event_Brake_Status_Callback(const std_msgs::msg::UInt32::SharedPtr status)
{
    Speaker_Led::Instance()->brake_ = (status->data == 0 || status->data == 2) ? 1 : 0;
}

void Wifi_Config_Callback(const std_msgs::msg::UInt8::SharedPtr config)
{
    Speaker_Led::Instance()->wificonfig = (config->data == 0) ? 1 : 2; // 1: 连接成功， 2：连接失败
}

void Mqtt_Status_Callback(const std_msgs::msg::UInt8::SharedPtr status)
{
    Speaker_Led::Instance()->mqttstatus = status->data;
    Speaker_Led::Instance()->Update_net_status();
}

void Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr speed)
{
    // 旋转速度大于某个值认为是旋转
    if (speed->angular.z >= 0.25)  // 左转
    {
        Speaker_Led::Instance()->rotatestatus = 1;
    }
    else if (speed->angular.z <= -0.25)  // 右转
    {
        Speaker_Led::Instance()->rotatestatus = 2;
    }
    else  //直行
    {
        Speaker_Led::Instance()->rotatestatus = 0;
    }
}




double filteredAngularVelocity = 0.0;
double alpha = 0.2; // 平滑参数，根据需要调整
double angularThreshold = 0.05; // 设置阈值，根据需要调整

double filteredAngularVelocity_voice = 0.0;
double alpha_voice = 0.1; // 平滑参数，根据需要调整
double angularThreshold_voice = 0.15; // 设置阈值，根据需要调整

void Odom_Callback(const nav_msgs::msg::Odometry::SharedPtr msg)
{
    // RCLCPP_INFO(node->get_logger(),"OdometryMsg::%f",msg->twist.twist.angular.z);

    double angularVelocity = msg->twist.twist.angular.z;
    // 一阶滤波
    filteredAngularVelocity = (1 - alpha) * filteredAngularVelocity + alpha * angularVelocity;
    filteredAngularVelocity_voice = (1 - alpha_voice) * filteredAngularVelocity_voice + alpha_voice * angularVelocity;

    // RCLCPP_WARN(node->get_logger(),"filteredAngularVelocity::%f",filteredAngularVelocity);
    

    if (filteredAngularVelocity >= angularThreshold)  // 左转
    {
        Speaker_Led::Instance()->rotatestatus = 1;
    }
    else if (filteredAngularVelocity <= -angularThreshold)  // 右转
    {
        Speaker_Led::Instance()->rotatestatus = 2;
    }
    else
    {
        Speaker_Led::Instance()->rotatestatus = 0;
    }

    if (filteredAngularVelocity_voice >= angularThreshold_voice)  // 左转
    {
        RCLCPP_WARN(node->get_logger(),"filteredAngularVelocity_voice----left----::%f",filteredAngularVelocity_voice);
        Speaker_Led::Instance()->rotatestatus_voice = 1;
    }
    else if (filteredAngularVelocity_voice <= -angularThreshold_voice)  // 右转
    {
        RCLCPP_WARN(node->get_logger(),"filteredAngularVelocity_voice----right----::%f",filteredAngularVelocity_voice);
        Speaker_Led::Instance()->rotatestatus_voice = 2;
    }
    else if (abs(filteredAngularVelocity_voice) < 0.001) 
    {
        RCLCPP_WARN(node->get_logger(),"filteredAngularVelocity_voice----0----::%f",filteredAngularVelocity_voice);
        Speaker_Led::Instance()->rotatestatus_voice = 0;
    }


}

void Speaker_Action_Callback(const std_msgs::msg::UInt8::SharedPtr action)
{
    Speaker_Led::Instance()->speakeraction = action->data;
}

void Initpose_Status_Callback(const std_msgs::msg::String::SharedPtr status)
{
    if (status->data == "OK")  // 初始定位成功
    {
        Speaker_Led::Instance()->initposestatus = 1;
    }
    else if (status->data == "NG")  // 初始定位失败
    {
        Speaker_Led::Instance()->initposestatus = 2;
    }
}

/*
msgs.data:
{
    "type":"task_status",
    "data": {
        "task_status": {
            "now_task_id":"",
            "status":"ERROR",
            "desc": "stopReason=10&stopReasonDesc=NAV_POSEJUMP",
            "action_index":"",
            "action_type":""
        }
    }
}
*/
// void Process_State_Callback(const std_msgs::msg::String::SharedPtr state)
void TaskStatusCallback(const std_msgs::msg::String::SharedPtr msgs)
{
    Json::Value root;

    RCLCPP_INFO_STREAM(node->get_logger(),"TaskStatusCallback msgs data: " << msgs->data);
    if (praseJsonString(msgs->data, root) == false)
    {
        return;
    }

    std::string desc   = root["data"]["task_status"]["desc"].asString();
    std::string cur_process_state = getErrorReasonState(desc);

    if (g_process_state_list.find(cur_process_state) != g_process_state_list.end())
    {
        Speaker_Led::Instance()->processstate |= g_process_state_list[cur_process_state];
    }
    else if (cur_process_state == "ADJUST_ABORT" && g_ubt_laststate == "UBT_RECHARGE")  // 对桩失败
    {
        Speaker_Led::Instance()->processstate |= (1 << 5);
    }
}

void Power_State_Callback(const std_msgs::msg::String::SharedPtr state)
{
    if (state->data == "LIMIT")  // 极限电量
    {
        Speaker_Led::Instance()->lowerpower = 2;
    }
    else if (state->data == "LOWER")  // 低电量
    {
        Speaker_Led::Instance()->lowerpower = 1;
    }
    else  // 正常电量
    {
        Speaker_Led::Instance()->lowerpower = 0;
    }
}

void ubt_state_o_obstacle_alarm_Callback(const std_msgs::msg::String::SharedPtr alarm)
{
    Speaker_Led::Instance()->obstaclealarm = 1;
}

void Nav_Speed_Limit_Callback(const std_msgs::msg::UInt8::SharedPtr state)
{
    Speaker_Led::Instance()->navspeedlimit = state->data;
}

void Stop_Carto_Callback(const std_msgs::msg::String::SharedPtr msg)
{
    if (msg->data == "start_carto") //开始建图
    {
        Speaker_Led::Instance()->mapstatus = 1;
    }
    if (msg->data == "stop_carto") //结束建图
    {
        Speaker_Led::Instance()->mapstatus = 2;
    }
}

void Action_speaker_Callback(const std_msgs::msg::UInt8::SharedPtr msg)
{
    if(msg->data == 1)
    {
        Speaker_Led::Instance()->actionWarn = 1;
    }
}

void Init_find_rfid_Callback(const std_msgs::msg::String::SharedPtr msg)
{
    if(msg->data == "1")
    {
        Speaker_Led::Instance()->autoFindState = 1;
    }

}


void Joystick_Status_Callback(const std_msgs::msg::Bool::SharedPtr msgs)
{
    if (msgs->data)
    {
        Speaker_Led::Instance()->joystickconnect = 1;
    }
    else
    {
        Speaker_Led::Instance()->joystickconnect = 0;
    }
}

void Ui_Config_Update_Callback(const std_msgs::msg::String::SharedPtr update)
{
    std::string test = " ";
    node->declare_parameter<std::string>("volume","");
    test = node->get_parameter("volume").as_string();
    Speaker_Led::Instance()->g_VoiceVolume = atoi(test.c_str());

    RCLCPP_INFO(node->get_logger(),"g_VoiceVolume = %d", Speaker_Led::Instance()->g_VoiceVolume);
}

void Charging_State_Callback(const std_msgs::msg::UInt32::SharedPtr msg)
{
    if (msg->data == 0x04 && Speaker_Led::Instance()->chargestatus == 1)
    {
        Speaker_Led::Instance()->chargefull = 1; // 充电状态,充电完成(满电)
    }
    else
    {
        Speaker_Led::Instance()->chargefull = 0;
    }
}

/***********************************thread fun********************************************/
void Topic_Sub_Thread()
{
    rclcpp::spin(node);
}

void Led_Update_Thread()
{
    rclcpp::Rate loop_rate(10);

    while (!g_StopThread)
    {
        Speaker_Led::Instance()->Led_Update();
        loop_rate.sleep();
    }
}

void UI_state_Callback(const std_msgs::msg::String::SharedPtr msg)
{
    if(msg->data == "mission_manual_pause")
    {
        Speaker_Led::Instance()->task_start_pause = 1;
    }
    else if(msg->data == "mission_manual_recover")
    {
        Speaker_Led::Instance()->task_start_pause = 2;
    }
}


int main(int argc,char** argv)
{
    rclcpp::init(argc, argv);
    node =  std::make_shared<rclcpp::Node>("speaker_led");

    rclcpp::Rate loop_rate(2);

    RCLCPP_INFO(node->get_logger(),"ubt_speaker_led v3.5.3 2021-3-26");

    Speaker_Led::Instance()->voice_ctl_pub = node->create_publisher<std_msgs::msg::UInt32MultiArray>("Voicectl", 1);
    Speaker_Led::Instance()->led_ctl_pub   = node->create_publisher<std_msgs::msg::UInt32MultiArray>("led_ctrl", 10);

    std::string test = " ";
    node->declare_parameter<std::string>("volume","");
    test = node->get_parameter("volume").as_string();
    Speaker_Led::Instance()->g_VoiceVolume = atoi(test.c_str());

   // rclcpp::Subscription<std_msgs::msg::String>::SharedPtr state_sub = node->create_subscription<std_msgs::msg::String>("state", 1, std::bind(State_Callback, std::placeholders::_1));
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr state_status_sub = node->create_subscription<std_msgs::msg::String>("ubt_state_status", 10, std::bind(StateStatusCallback, std::placeholders::_1));
    rclcpp::Subscription<ubt_interface::msg::WifiInfo>::SharedPtr wifi_info_sub = node->create_subscription<ubt_interface::msg::WifiInfo>("wifi_info", 1, std::bind(Wifi_Info_Callback, std::placeholders::_1));
    rclcpp::Subscription<std_msgs::msg::UInt8>::SharedPtr pb_status_sub = node->create_subscription<std_msgs::msg::UInt8>("power_event", 1, std::bind(Pb_Status_Callback, std::placeholders::_1));
    rclcpp::Subscription<std_msgs::msg::UInt32>::SharedPtr ble_status_sub = node->create_subscription<std_msgs::msg::UInt32>("ble_conn_state", 10, std::bind(Ble_Status_Callback, std::placeholders::_1));
    rclcpp::Subscription<std_msgs::msg::UInt32>::SharedPtr event_emergency_sub = node->create_subscription<std_msgs::msg::UInt32>("event_emergency", 1, std::bind(Event_Emergency_Status_Callback, std::placeholders::_1));
    rclcpp::Subscription<std_msgs::msg::UInt32>::SharedPtr event_collision_sub = node->create_subscription<std_msgs::msg::UInt32>("event_collision", 1, std::bind(Event_Collision_Status_Callback, std::placeholders::_1));
    rclcpp::Subscription<std_msgs::msg::UInt32>::SharedPtr event_brake_sub = node->create_subscription<std_msgs::msg::UInt32>("event_brake", 1, std::bind(Event_Brake_Status_Callback, std::placeholders::_1));
    rclcpp::Subscription<std_msgs::msg::UInt32>::SharedPtr lifting_jack_state = node->create_subscription<std_msgs::msg::UInt32>("lifting_jack_state", 1, std::bind(Event_Brake_Status_Callback, std::placeholders::_1));//手动模式使用之前的解抱闸
    rclcpp::Subscription<std_msgs::msg::UInt8>::SharedPtr wifi_config_sub = node->create_subscription<std_msgs::msg::UInt8>("wifi_config_status", 1, std::bind(Wifi_Config_Callback, std::placeholders::_1));
    rclcpp::Subscription<std_msgs::msg::UInt8>::SharedPtr mqtt_status_sub = node->create_subscription<std_msgs::msg::UInt8>("mqtt_status", 1, std::bind(Mqtt_Status_Callback, std::placeholders::_1));
    // rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub = node->create_subscription<geometry_msgs::msg::Twist>("cmd_vel", 1, std::bind(Cmd_Vel_Callback, std::placeholders::_1));
    // rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr direct_cmd_vel_sub = node->create_subscription<geometry_msgs::msg::Twist>("direct_cmd_vel", 1, std::bind(Direct_Cmd_Vel_Callback, std::placeholders::_1));
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub = node->create_subscription<nav_msgs::msg::Odometry>("odom", 1, std::bind(Odom_Callback, std::placeholders::_1));
    rclcpp::Subscription<std_msgs::msg::UInt8>::SharedPtr speaker_action_sub = node->create_subscription<std_msgs::msg::UInt8>("speaker_action", 1, std::bind(Speaker_Action_Callback, std::placeholders::_1));
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr initpose_status_sub = node->create_subscription<std_msgs::msg::String>("inital_state", 1, std::bind(Initpose_Status_Callback, std::placeholders::_1));
    //rclcpp::Subscription<std_msgs::msg::String>::SharedPtr process_state_sub = node->create_subscription<std_msgs::msg::String>("process_state", 10, std::bind(Process_State_Callback, std::placeholders::_1));  // 任务执行过程状态反馈
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr task_status_sub = node->create_subscription<std_msgs::msg::String>("task_status", 10, std::bind(TaskStatusCallback, std::placeholders::_1)); // 任务执行过程状态反馈
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr power_state_sub = node->create_subscription<std_msgs::msg::String>("power_state", 1, std::bind(Power_State_Callback, std::placeholders::_1));
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr ubt_state_o_obstacle_alarm_sub = node->create_subscription<std_msgs::msg::String>("ubt_state_o_obstacle_alarm", 1, std::bind(ubt_state_o_obstacle_alarm_Callback, std::placeholders::_1));
    rclcpp::Subscription<std_msgs::msg::UInt8>::SharedPtr nav_speed_limit_sub = node->create_subscription<std_msgs::msg::UInt8>("nav_speed_limit", 1, std::bind(Nav_Speed_Limit_Callback, std::placeholders::_1));
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr ui_config_update_sub = node->create_subscription<std_msgs::msg::String>("ui_config_update",10,std::bind(Ui_Config_Update_Callback, std::placeholders::_1));
    rclcpp::Subscription<std_msgs::msg::UInt32>::SharedPtr charging_state_sub = node->create_subscription<std_msgs::msg::UInt32>("charging_state", 10, std::bind(Charging_State_Callback, std::placeholders::_1));
    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr joystick_status_sub = node->create_subscription<std_msgs::msg::Bool>("/joystick_connect", 1, std::bind(Joystick_Status_Callback, std::placeholders::_1));
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr stop_carto_sub = node->create_subscription<std_msgs::msg::String>("/ui_stop_carto", 1, std::bind(Stop_Carto_Callback, std::placeholders::_1)); //启停建图任务
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr init_find_rfid_sub = node->create_subscription<std_msgs::msg::String>("/init_find_rfid", 10, std::bind(Init_find_rfid_Callback, std::placeholders::_1));
    rclcpp::Subscription<std_msgs::msg::UInt8>::SharedPtr action_speaker_sub = node->create_subscription<std_msgs::msg::UInt8>("/action_speaker", 10, std::bind(Action_speaker_Callback, std::placeholders::_1));
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr ui_state_sub = node->create_subscription<std_msgs::msg::String>("/ui_state", 10, std::bind(UI_state_Callback, std::placeholders::_1));
    boost::thread topic_sub_thread(Topic_Sub_Thread);
    boost::thread led_update_thread(Led_Update_Thread);

    //  speak_allow_timer = nh.createTimer(ros::Duration(1), Speak_Allow_Timer_Callback);

    while (rclcpp::ok())
    {
        Speaker_Led::Instance()->Speaker_Update();
        rclcpp::spin_some(node);
        loop_rate.sleep();
    }

    g_StopThread = true;
    topic_sub_thread.join();
    led_update_thread.join();

    rclcpp::shutdown();
    return 0;
}


